// --------------------------------------------------------------------------------------------------------------------
// <copyright file="Matrix2.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Numerics
{
    using System;

    /// <summary>
    /// A 2x2 matrix class
    /// </summary>
    public struct Matrix2
    {
        /// <summary>
        /// The first matrix entry
        /// </summary>
        private double m00;

        /// <summary>
        /// The first matrix entry
        /// </summary>
        private double m01;

        /// <summary>
        /// The first matrix entry
        /// </summary>
        private double m10;

        /// <summary>
        /// The first matrix entry
        /// </summary>
        private double m11;

        /// <summary>
        /// Initializes a new instance of the <see cref="Matrix2"/> struct.
        /// </summary>
        /// <param name="value">Value to set all entries to</param>
        public Matrix2(double value)
        {
            this.m00 = value;
            this.m01 = value;
            this.m10 = value;
            this.m11 = value;
        }

        /// <summary>
        /// Initializes a new instance of the <see cref="Matrix2"/> struct.
        /// </summary>
        /// <param name="m">Matrix to be copied</param>
        public Matrix2(Matrix2 m)
        {
            this.m00 = m.m00;
            this.m01 = m.m01;
            this.m10 = m.m10;
            this.m11 = m.m11;
        }

        /// <summary>
        /// Initializes a new instance of the <see cref="Matrix2"/> struct.
        /// by supplying all the elements
        /// </summary>
        /// <param name="m00">M00 entry</param>
        /// <param name="m01">M01 entry</param>
        /// <param name="m10">M10 entry</param>
        /// <param name="m11">M11 entry</param>
        public Matrix2(double m00, double m01, double m10, double m11)
        {
            this.m00 = m00;
            this.m01 = m01;
            this.m10 = m10;
            this.m11 = m11;
        }

        /// <summary>
        /// Gets or sets the first matrix entry
        /// </summary>
        public double M00
        {
            get
            {
                return this.m00;
            }

            set
            {
                this.m00 = value;
            }
        }

        /// <summary>
        /// Gets or sets the second matrix entry
        /// </summary>
        public double M01
        {
            get
            {
                return this.m01;
            }

            set
            {
                this.m01 = value;
            }
        }

        /// <summary>
        /// Gets or sets the third matrix entry
        /// </summary>
        public double M10
        {
            get
            {
                return this.m10;
            }

            set
            {
                this.m10 = value;
            }
        }

        /// <summary>
        /// Gets or sets the fourth matrix entry
        /// </summary>
        public double M11
        {
            get
            {
                return this.m11;
            }

            set
            {
                this.m11 = value;
            }
        }

        /// <summary>
        /// Create an identity matrix
        /// </summary>
        /// <returns>Identity matrix</returns>
        public static Matrix2 Identity()
        {
            Matrix2 result = new Matrix2();
            result.m00 = 1.0;
            result.m11 = 1.0;
            return result;
        }

        /// <summary>
        /// Transpose a matrix
        /// </summary>
        /// <param name="m">Input matrix</param>
        /// <returns>Transposed matrix</returns>
        public static Matrix2 Transpose(Matrix2 m)
        {
            Matrix2 transpose = new Matrix2();
            transpose.m00 = m.m00;
            transpose.m11 = m.m11;
            transpose.m01 = m.m10;
            transpose.m10 = m.m01;
            return transpose;
        }

        /// <summary>
        /// Invert a matrix
        /// </summary>
        /// <param name="m">Input matrix</param>
        /// <returns>The matrix inverse</returns>
        public static Matrix2 Inverse(Matrix2 m)
        {
            Matrix2 inverse = new Matrix2(0.0);
            double determinant = (m.m00 * m.m11) - (m.m01 * m.m10);
            if (determinant != 0.0)
            {
                inverse.m00 = m.m11 / determinant;
                inverse.m01 = -1.0 * (m.m01 / determinant);
                inverse.m10 = -1.0 * (m.m10 / determinant);
                inverse.m11 = m.m00 / determinant;
            }

            return inverse;
        }

        /// <summary>
        /// The matrix multiplication operator
        /// </summary>
        /// <param name="m1">Left side</param>
        /// <param name="m2">Right side</param>
        /// <returns>The product result</returns>
        public static Matrix2 operator *(Matrix2 m1, Matrix2 m2)
        {
            Matrix2 result = new Matrix2();
            result.m00 = (m1.m00 * m2.m00) + (m1.m01 * m2.m10);
            result.m01 = (m1.m00 * m2.m01) + (m1.m01 * m2.m11);
            result.m10 = (m1.m10 * m2.m00) + (m1.m11 * m2.m10);
            result.m11 = (m1.m10 * m2.m01) + (m1.m11 * m2.m11);

            return result;
        }

        /// <summary>
        /// The matrix with vector multiplication operator
        /// </summary>
        /// <param name="m">Left side</param>
        /// <param name="v">Right side</param>
        /// <returns>The product result</returns>
        public static Vector2 operator *(Matrix2 m, Vector2 v)
        {
            Vector2 result = new Vector2();
            result.X = (m.m00 * v.X) + (m.m01 * v.Y);
            result.Y = (m.m10 * v.X) + (m.m11 * v.Y);
            
            return result;
        }

        /// <summary>
        /// The matrix addition operator
        /// </summary>
        /// <param name="m1">Left side</param>
        /// <param name="m2">Right side</param>
        /// <returns>The matrix sum</returns>
        public static Matrix2 operator +(Matrix2 m1, Matrix2 m2)
        {
            Matrix2 result = new Matrix2();
            result.m00 = m1.m00 + m2.m00;
            result.m01 = m1.m01 + m2.m01;
            result.m10 = m1.m10 + m2.m10;
            result.m11 = m1.m11 + m2.m11;

            return result;
        }

        /// <summary>
        /// The matrix subtraction operator
        /// </summary>
        /// <param name="m1">Left side</param>
        /// <param name="m2">Right side</param>
        /// <returns>The operator result</returns>
        public static Matrix2 operator -(Matrix2 m1, Matrix2 m2)
        {
            Matrix2 result = new Matrix2();
            result.m00 = m1.m00 - m2.m00;
            result.m01 = m1.m01 - m2.m01;
            result.m10 = m1.m10 - m2.m10;
            result.m11 = m1.m11 - m2.m11;

            return result;
        }
    }
}
